/********************************************************************************
 * Copyright(c) 2020-2025 BST All rights reserved.
 * - Filename
 * - Author
 * - Version V1.0.0
 * - Date    2024/08/28
 * - Brief
 * - FunctionList:
 ******************************************************************************
 * History:
 *
 *
 *
 ******************************************************************************
 */
#include "LsLidarAdapter.h"
#include <sys/time.h>
#include "TimeUtil.h"

namespace lslidar
{

  using namespace lslidar;
  using namespace lidar;
  LsLidarAdapter::LsLidarAdapter(const uint32_t &id, const uint16_t &msop_port,
                                 const uint16_t &difop_port)
      : _id(id), _msopPort(msop_port), _difopPort(difop_port)
  {
    std::string info;
    info.append("Id:");
    info.append(std::to_string(_id));
    info.append(", MsopPort:");
    info.append(std::to_string(_msopPort));
    info.append(", DifoPort:");
    info.append(std::to_string(_difopPort));

    std::cout << "Lidar Impl create : " << info << std::endl;
  }

  LsLidarAdapter::~LsLidarAdapter()
  {
    std::cout << "LslidarAdapter Destroy..." << std::endl;
  }

  void LsLidarAdapter::start(
      std::function<void(std::shared_ptr<lpc::LidarProtoPointCloud> &&)> callback)
  {
    _isExit = false;
    _msop = std::make_shared<UdpClient>();
    _difop = std::make_shared<UdpClient>();
    _lidar = std::make_shared<LslidarCh128x1>();
    if (_msop->init(_msopPort) < 0)
      return;
    if (_difop->init(_difopPort) < 0)
      return;

    _lidar->start();
    _lidar->setLidarFrameCallback(std::bind(&LsLidarAdapter::pointsCallback, this,
                                            std::placeholders::_1,
                                            std::placeholders::_2));
    _msopRecvThread.reset(
        new std::thread(std::bind(&LsLidarAdapter::recvMsopLoop, this)));
    _difopRecvThread.reset(
        new std::thread(std::bind(&LsLidarAdapter::recvDifopLoop, this)));
    _packageThread.reset(
        new std::thread(std::bind(&LsLidarAdapter::processLoop, this, callback)));
  }

  void LsLidarAdapter::stop()
  {
    _lidar->stop();
    _isExit = true;
    _condition.notify_all();
    if (_msopRecvThread != nullptr && _msopRecvThread->joinable())
    {
      _msopRecvThread->join();
    }

    if (_difopRecvThread != nullptr && _difopRecvThread->joinable())
    {
      _difopRecvThread->join();
    }

    if (_packageThread != nullptr && _packageThread->joinable())
    {
      _packageThread->join();
    }
    std::cout << "LiarImpl Exit ..." << std::endl;
  }

  void LsLidarAdapter::processLoop(
      const std::function<void(std::shared_ptr<lpc::LidarProtoPointCloud> &&)>
          &callback)
  {

    while (!_isExit)
    {
      std::unique_lock<std::mutex> lck(_mtx);
      _condition.wait(lck,
                      [this]
                      { return _isExit || !_lidarFrameDeque.empty(); });
      if (_isExit)
        break;
      auto lidarFrame = _lidarFrameDeque.front();
      _lidarFrameDeque.pop_front();
      lck.unlock();
      if (_isExit)
        break;

      // if (TimeUtil::fps_calc_inc(_fps)) {
      //   std::cout << "[FPS]: lidar" << lidarFrame->sensor_id
      //             << ", point count: " << lidarFrame->point_count
      //             << " total fps:" << _fps.avg_fps
      //             << ", fps: " << _fps.ins_fps << std::endl;
      // }
      callback(std::move(lidarFrame));
    }
  }

  void LsLidarAdapter::recvDifopLoop()
  {
    uint8_t m_recvBuf[2048] = {0};
    while (!_isExit)
    {
      int m_recvLen = _difop->readData(m_recvBuf, sizeof(m_recvBuf));
      if (m_recvLen > 0)
      {
        _lidar->lidarDataArrived(m_recvBuf, m_recvLen);
      }
      else
      {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
      }
    }
  }

  void LsLidarAdapter::recvMsopLoop()
  {
    uint8_t m_recvBuf[2048] = {0};
    while (!_isExit)
    {
      int m_recvLen = _msop->readData(m_recvBuf, sizeof(m_recvBuf));
      if (m_recvLen > 0)
      {
        _lidar->lidarDataArrived(m_recvBuf, m_recvLen);
      }
      else
      {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
      }
    }
  }

  void LsLidarAdapter::pointsCallback(
      const uint64_t &tm, const std::vector<LidarPoint> &lidar_frame)
  {
    auto frame = std::make_shared<lpc::LidarProtoPointCloud>();
    frame->set_lidar_id(_id);
    frame->set_system_timestamp(TimeUtil::now_us());
    frame->set_timestamp(tm / 1000); // ns to us
    frame->set_seq(0);
    frame->set_height(lidar_frame.size());
    frame->set_width(1);
    frame->set_is_dense(false);

    for (int i = 0; i < (int)lidar_frame.size(); i++)
    {
      frame->add_data(lidar_frame[i].x);
      frame->add_data(lidar_frame[i].y);
      frame->add_data(lidar_frame[i].z);
      frame->add_data(lidar_frame[i].intensity);
    }

    {
      std::unique_lock<std::mutex> lck(_mtx);
      _lidarFrameDeque.emplace_back(frame);
    }
    _condition.notify_one();
  }
} // namespace lslidar